import pandas as pd
import matplotlib.pyplot as plt
from sklearn.linear_model import LinearRegression
import numpy as np
import yaml
from matplotlib.widgets import Button
def parse_ksxt_line(line):
    """Parse a single KSXT line into its components."""
    line = line[2:len(line) - 1]
    parts = line.strip().split(',')
    try:
        lon = float(parts[2])
        lat = float(parts[3])
        return {'Longitude': lon, 'Latitude': lat}
    except (IndexError, ValueError):
        return None

def read_ksxt_file(file_path):
    """Read and parse all valid KSXT lines from the file, removing duplicates."""
    data = []
    seen_points = set()  # To keep track of unique points.
    with open(file_path, 'r') as file:
        for line in file:
            parsed_data = parse_ksxt_line(line)
            if parsed_data is not None:
                point_tuple = (parsed_data['Longitude'], parsed_data['Latitude'])
                if point_tuple not in seen_points:
                    seen_points.add(point_tuple)
                    data.append(parsed_data)
    return pd.DataFrame(data)

selected_lines = []  # List to hold selected lines
start_point = None   # Variable to hold the start point of a line

continue_drawing = True  # 控制是否以上次的终点作为起点
# def onclick(event):
#     """Event handler for mouse click events on the plot."""
#     global ax, fig, start_point, continue_drawing
#     if event.inaxes is not None:
#         clicked_lon, clicked_lat = event.xdata, event.ydata
        
#         # Toggle continue_drawing state when right mouse button is clicked
#         if event.button == 3:  # Right mouse button
#             continue_drawing = not continue_drawing
#             print(f"Continue drawing set to: {continue_drawing}")
#             return

#         if start_point is None or not continue_drawing:
#             # This is the first click or user chose not to continue, set it as the start point or a new line
#             start_point = {'Longitude': clicked_lon, 'Latitude': clicked_lat}
#             print(f"Start position set: Longitude={clicked_lon}, Latitude={clicked_lat}")
            
#             # Add blue marker at the start position
#             ax.scatter(clicked_lon, clicked_lat, s=50, c='blue', marker='o')
#             fig.canvas.draw_idle()
#         else:
#             # This is the second click, draw the line and prepare for the next line
            
#             # Decide whether to keep the latitude or longitude of the start point
#             if abs(clicked_lon - start_point['Longitude']) > abs(clicked_lat - start_point['Latitude']):
#                 # Horizontal line (parallel to X-axis)
#                 end_point = {'Longitude': clicked_lon, 'Latitude': start_point['Latitude']}
#             else:
#                 # Vertical line (parallel to Y-axis)
#                 end_point = {'Longitude': start_point['Longitude'], 'Latitude': clicked_lat}

#             print(f"End position set: Longitude={end_point['Longitude']}, Latitude={end_point['Latitude']}")
            
#             # Draw the line between start and end points
#             line = ax.plot([start_point['Longitude'], end_point['Longitude']],
#                            [start_point['Latitude'], end_point['Latitude']], c='red')
#             fig.canvas.draw_idle()
            
#             # Save the line (pair of points) to the list
#             selected_lines.append((start_point, end_point))
            
#             # Optionally save to file after each line is drawn
#             save_selected_lines_to_file('selected_lines.txt')
            
#             # Prepare for the next line
#             if continue_drawing:
#                 start_point = end_point  # Use the current end point as the start point for the next line
#             else:
#                 start_point = None  # Start a new line from the next click

def onclick(event):
    """Event handler for mouse click events on the plot."""
    global ax, fig, start_point, continue_drawing
    if event.inaxes is not None:
        clicked_lon, clicked_lat = event.xdata, event.ydata

        # Toggle continue_drawing state when right mouse button is clicked
        if event.button == 3:  # Right mouse button
            continue_drawing = not continue_drawing
            print(f"Continue drawing set to: {continue_drawing}")
            return

        if start_point is None or not continue_drawing:
            # This is the first click or user chose not to continue, set it as the start point or a new line
            start_point = {'Longitude': clicked_lon, 'Latitude': clicked_lat}
            print(f"Start position set: Longitude={clicked_lon}, Latitude={clicked_lat}")

            # Add blue marker at the start position
            ax.scatter(clicked_lon, clicked_lat, s=50, c='blue', marker='o')
            fig.canvas.draw_idle()
        else:
            # This is the second click, draw the line directly between start and end points
            end_point = {'Longitude': clicked_lon, 'Latitude': clicked_lat}

            print(f"End position set: Longitude={end_point['Longitude']}, Latitude={end_point['Latitude']}")

            # Draw the line between start and end points
            line = ax.plot([start_point['Longitude'], end_point['Longitude']],
                           [start_point['Latitude'], end_point['Latitude']], c='red')
            fig.canvas.draw_idle()

            # Save the line (pair of points) to the list
            selected_lines.append((start_point, end_point))

            # Optionally save to file after each line is drawn
            save_selected_lines_to_file('selected_lines.txt')

            # Prepare for the next line
            if continue_drawing:
                start_point = end_point  # Use the current end point as the start point for the next line
            else:
                start_point = None  # Start a new line from the next click


def save_selected_lines_to_file(filename):
    """Save the selected lines to a file."""
    with open(filename, 'a') as f:
        for line in selected_lines[-1:]:
            f.write(f"{line[0]['Longitude']},{line[0]['Latitude']},{line[1]['Longitude']},{line[1]['Latitude']}\n")
    print(f"Selected line saved to {filename}")

def plot_points(df):
    """Plot points from DataFrame and enable clicking on the plot to get GPS positions."""
    global fig, ax
    fig, ax = plt.subplots(figsize=(10, 8))
    ax.scatter(df['Longitude'], df['Latitude'], s=10, c='blue', marker='o')
    plt.title('KSXT Data Points Without Duplicates')
    plt.xlabel('Longitude')
    plt.ylabel('Latitude')
    plt.grid(True)
    
    # Connect the click event to our handler
    cid = fig.canvas.mpl_connect('button_press_event', onclick)

    plt.show()
if __name__ == "__main__":
    ksxt_df = read_ksxt_file(r'C:\Users\17963\Desktop\ros_workspace\navigation2_tutorials\my_test_dir\gps\rtk_data_0.txt')
    print(ksxt_df)
    plot_points(ksxt_df)